
#ifndef VECTOR4_H_
#define VECTOR4_H_

#include "libmath.h"
#include "Vector2.h"

namespace p2 {

template<typename Real>
class Vector4
{
public:
	inline Vector4(); // uninitialized
	inline Vector4(Vector4<Real> const& v);
	inline Vector4(Real x, Real y, Real z, Real w);
	inline Vector4(Real x, Real y, Real z);
	inline Vector4(Vector2<Real> const& v);	// { vx, vy, 0, 1 }
	inline Vector4(Real* v);
	inline ~Vector4();

	Vector4<Real>& operator=(const Vector4<Real>& v);

	// access
	inline Real X() const;
	inline Real& X();
	inline Real Y() const;
	inline Real& Y();
	inline Real Z() const;
	inline Real& Z();
	inline Real W() const;
	inline Real& W();

	// operators
	inline Vector4<Real> operator+(const Vector4<Real>& v) const;
	inline Vector4<Real> operator-(const Vector4<Real>& v) const;
	inline Vector4<Real> operator*(Real scalar) const;
	inline Vector4<Real> operator/(Real scalar) const;
	inline Vector4<Real> operator-() const;
	inline Vector4<Real>& operator+=(const Vector4<Real>& v);
	inline Vector4<Real>& operator-=(const Vector4<Real>& v);
	inline Vector4<Real>& operator*=(Real scalar);
	inline Vector4<Real>& operator/=(Real scalar);
	inline Real const& operator[](int i) const;
	inline Real& operator[](int i);

	// operations
	inline Real length() const;
	static inline Real length(Vector4<Real> v);
	inline Real invLength() const;
	inline Real squaredLength() const;
	inline Real dot(Vector4<Real> v) const;
	static inline Real dot(Vector4<Real> v1, Vector4<Real> v2);
	static inline Vector4<Real> normalize(Vector4<Real> v);
	inline void normalize();
	static inline Vector4<Real> cross(const Vector4<Real>& a, const Vector4<Real>& b);
	static inline Vector4<Real> makeNormal(Vector4<Real> const& v0,
		Vector4<Real> const& v1);

	inline Vector4<Real> clamp(Vector4<Real> min, Vector4<Real> max);
	static inline Vector4<Real> clamp(Vector4<Real> v, Real len);

	// constants
	static const Vector4 ZERO; // (0,0,0,1)
	static const Vector4 UNIT_X; // (1,0,0,1)
	static const Vector4 UNIT_Y; // (0,1,0,1)
	static const Vector4 UNIT_Z; // (0,0,1,1)
	static const Vector4 ONE; // (1,1,1,1)

protected:
	Real _t[4];
};

template<typename Real>
inline Vector4<Real> operator*(Real scalar, const Vector4<Real>& v);

template<typename Real>
std::ostream& operator<<(std::ostream& outFile, const Vector4<Real>& v);

template<typename Real>
Vector4<Real>::Vector4()
{
}

template<typename Real>
Vector4<Real>::Vector4(const Vector4<Real>& v)
{
	_t[0] = v._t[0];
	_t[1] = v._t[1];
	_t[2] = v._t[2];
	_t[3] = v._t[3];
}

template<typename Real>
Vector4<Real>::Vector4(Real x, Real y, Real z, Real w)
{
	_t[0] = x;
	_t[1] = y;
	_t[2] = z;
	_t[3] = w;
}

template<typename Real>
Vector4<Real>::Vector4(Real x, Real y, Real z)
{
	_t[0] = x;
	_t[1] = y;
	_t[2] = z;
	_t[3] = (Real)1;
}

template<typename Real>
Vector4<Real>::Vector4(const Vector2<Real>& v)
{
	_t[0] = v[0];
	_t[1] = v[1];
	_t[2] = (Real)0;
	_t[3] = (Real)1;
}

template<typename Real>
Vector4<Real>::Vector4(Real* v)
{
	_t[0] = v[0];
	_t[1] = v[1];
	_t[2] = v[2];
	_t[3] = v[3];
}

template<typename Real>
Vector4<Real>::~Vector4()
{
}

template<typename Real>
Vector4<Real>& Vector4<Real>::operator=(const Vector4<Real>& v)
{
	_t[0] = v._t[0];
	_t[1] = v._t[1];
	_t[2] = v._t[2];
	_t[3] = v._t[3];
	return *this;
}

template<typename Real>
inline Real const& Vector4<Real>::operator[](int i) const
{
	return _t[i];
}

template<typename Real>
inline Real& Vector4<Real>::operator[](int i)
{
	return _t[i];
}

template<typename Real>
inline Real Vector4<Real>::X() const
{
	return _t[0];
}

template<typename Real>
inline Real& Vector4<Real>::X()
{
	return _t[0];
}

template<typename Real>
inline Real Vector4<Real>::Y() const
{
	return _t[1];
}

template<typename Real>
inline Real& Vector4<Real>::Y()
{
	return _t[1];
}

template<typename Real>
inline Real Vector4<Real>::Z() const
{
	return _t[2];
}

template<typename Real>
inline Real& Vector4<Real>::Z()
{
	return _t[2];
}

template<typename Real>
inline Real Vector4<Real>::W() const
{
	return _t[3];
}

template<typename Real>
inline Real& Vector4<Real>::W()
{
	return _t[3];
}

template<typename Real>
inline Vector4<Real> Vector4<Real>::operator+(const Vector4<Real>& v) const
{
	return Vector4<Real>(_t[0] + v._t[0], _t[1] + v._t[1], _t[2] + v._t[2]);
}

template<typename Real>
inline Vector4<Real> Vector4<Real>::operator-(const Vector4<Real>& v) const
{
	return Vector4<Real>(_t[0] - v._t[0], _t[1] - v._t[1], _t[2] - v._t[2]);
}

template<typename Real>
inline Vector4<Real> Vector4<Real>::operator*(Real scalar) const
{
	return Vector4<Real>(scalar * _t[0], scalar * _t[1], scalar * _t[2]);
}

template<typename Real>
inline Vector4<Real> Vector4<Real>::operator/(Real scalar) const
{
	Vector4<Real> result;

	if (scalar != (Real) 0) {
		Real invScalar = ((Real) 1) / scalar;
		result._t[0] = invScalar * _t[0];
		result._t[1] = invScalar * _t[1];
		result._t[2] = invScalar * _t[2];
	} else {
		result._t[0] = Math<Real>::MAX_REAL;
		result._t[1] = Math<Real>::MAX_REAL;
		result._t[2] = Math<Real>::MAX_REAL;
	}

	return result;
}

template<typename Real>
inline Vector4<Real> Vector4<Real>::operator-() const
{
	return Vector4<Real>(-_t[0], -_t[1], -_t[2]);
}

template<typename Real>
inline Vector4<Real>& Vector4<Real>::operator+=(const Vector4<Real>& v)
{
	_t[0] += v._t[0];
	_t[1] += v._t[1];
	_t[2] += v._t[2];
	return *this;
}

template<typename Real>
inline Vector4<Real>& Vector4<Real>::operator-=(const Vector4<Real>& v)
{
	_t[0] -= v._t[0];
	_t[1] -= v._t[1];
	_t[2] -= v._t[2];
	return *this;
}

template<typename Real>
inline Vector4<Real>& Vector4<Real>::operator*=(Real scalar)
{
	_t[0] *= scalar;
	_t[1] *= scalar;
	_t[2] *= scalar;
	return *this;
}

template<typename Real>
inline Vector4<Real>& Vector4<Real>::operator/=(Real scalar)
{
	if (scalar != (Real) 0) {
		Real invScalar = ((Real) 1) / scalar;
		_t[0] *= invScalar;
		_t[1] *= invScalar;
		_t[2] *= invScalar;
	} else {
		_t[0] = Math<Real>::MAX_REAL;
		_t[1] = Math<Real>::MAX_REAL;
		_t[2] = Math<Real>::MAX_REAL;
	}
	return *this;
}

template<typename Real>
inline Real Vector4<Real>::length() const
{
	return Math<Real>::Sqrt(_t[0] * _t[0] + _t[1] * _t[1] + _t[2] * _t[2]);
}

template<typename Real>
inline Real Vector4<Real>::length(Vector4<Real> v)
{
	return Math<Real>::Sqrt(v._t[0] * v._t[0] + v._t[1] * v._t[1] + v._t[2] * v._t[2]);
}

template<typename Real>
inline Real Vector4<Real>::invLength() const
{
	return Math<Real>::InvSqrt(_t[0] * _t[0] + _t[1] * _t[1] + _t[2] * _t[2]);
}

template<typename Real>
inline Real Vector4<Real>::squaredLength() const
{
	return _t[0] * _t[0] + _t[1] * _t[1] + _t[2] * _t[2];
}

template<typename Real>
inline Real Vector4<Real>::dot(Vector4<Real> v) const
{
	return (_t[0] * v._t[0] + _t[1] * v._t[1] + _t[2] * v._t[2]);
}

template<typename Real>
inline Real Vector4<Real>::dot(Vector4<Real> v1, Vector4<Real> v2)
{
	return (v1._t[0] * v2._t[0] + v1._t[1] * v2._t[1] + v1._t[2] * v2._t[2]);
}

template<typename Real>
inline void Vector4<Real>::normalize()
{
	Real len2 = squaredLength();
	if (len2 > Math<Real>::ZERO_TOLERANCE2) {
		Real invLen = Math<Real>::InvSqrt(len2);
		_t[0] *= invLen;
		_t[1] *= invLen;
		_t[2] *= invLen;
	} else {
		_t[0] = (Real) 0;
		_t[1] = (Real) 0;
		_t[2] = (Real) 0;
	}
}

template<typename Real>
inline Vector4<Real> Vector4<Real>::normalize(Vector4<Real> v)
{
	Real len2 = v.squaredLength();
	if (len2 > Math<Real>::ZERO_TOLERANCE2) {
		Real invLen = Math<Real>::InvSqrt(len2);
		v._t[0] *= invLen;
		v._t[1] *= invLen;
		v._t[2] *= invLen;
	} else {
		v._t[0] = (Real) 0;
		v._t[1] = (Real) 0;
		v._t[2] = (Real) 0;
	}
	return v;
}

template<typename Real>
inline Vector4<Real> Vector4<Real>::cross(const Vector4<Real>& a, const Vector4<Real>& b)
{
	return Vector4<Real>(
		(a._t[1] * b._t[2]) - (b._t[1] * a._t[2]),
		(a._t[2] * b._t[0]) - (b._t[2] * a._t[0]),
		(a._t[0] * b._t[1]) - (b._t[0] * a._t[1])
	);
}

template<typename Real>
inline Vector4<Real> Vector4<Real>::makeNormal(Vector4<Real> const& v0, Vector4<Real> const& v1)
{
	Vector4<Real> n( ((v1 - v0).perp()) );
	n.normalize();
	return n;
}

template<typename Real>
inline Vector4<Real> Vector4<Real>::clamp(Vector4<Real> min, Vector4<Real> max)
{
	return Vector4<Real>(
		Math<Real>::Max(Math<Real>::Min(X(), max.X()), min.X()),
		Math<Real>::Max(Math<Real>::Min(Y(), max.Y()), min.Y()),
		Math<Real>::Max(Math<Real>::Min(Z(), max.Z()), min.Z())
    );
}

template<typename Real>
inline Vector4<Real> Vector4<Real>::clamp(Vector4<Real> v, Real len)
{
	return (Vector4<Real>::dot(v, v) > len * len) ? (Vector4<Real>::normalize(v) * len) : v;
}

template<typename Real>
inline Vector4<Real> operator*(Real scalar, const Vector4<Real>& v)
{
	return Vector4<Real>(scalar * v[0], scalar * v[1], scalar * v[2]);
}

}

#endif	//VECTOR4_H_
